import rclpy
from rclpy.node import Node
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from rclpy.time import Time
from geometry_msgs.msg import TransformStamped
class TFListenerPyNode(Node):
    def __init__(self):
        super().__init__('tf_listener_cpp_node')
        self.get_logger().info('TFListenerPyNode创建')
        self.buffer_=Buffer()
        self.listener_=TransformListener(self.buffer_,self)
        self.timer_=self.create_timer(2.0,self.on_timer)
    def on_timer(self):
        if self.buffer_.can_transform("camera","laser",Time()):
            ts: TransformStamped=self.buffer_.lookup_transform("camera","laser",Time())
            self.get_logger().info("父:%s 子:%s 相对便宜 %.2f  %.2f %.2f"%(ts.header.frame_id,
                                                                     ts.child_frame_id,
                                                                     ts.transform.translation.x,
                                                                     ts.transform.translation.y,
                                                                     ts.transform.translation.z))
        else:
            self.get_logger().error('无法进行转换')    
def main():
    rclpy.init()
    rclpy.spin(TFListenerPyNode())
    rclpy.shutdown()


if __name__ == '__main__':
    main()